# -*- coding: utf-8 -*-
import json
import logging
import os
import socket
import struct
import subprocess
import threading
import time
import signal
import re
from threading import Thread

import modbus_tk.defines as cst
import modbus_tk.modbus_tcp as modbus_tcp
from periphery import GPIO
from robot_gm import robot_control

import config

'''
####
Date : 2024-4-29
Notes: VSR实体控制器--server端

Test: 通讯格式
####
'''

robot_logger=logging.getLogger("Controler-Driver")
robot_logger.setLevel(logging.DEBUG)

algorithm_path = "/home/cat/gm/algorithm_run/"

class Controler_Server():
    def __init__(self, host = "0.0.0.0", port = 502):
        super().__init__()
        ### 获取本机的IP地址和名字
        host_name = socket.gethostname()
        self.host_add = socket.gethostbyname(host_name)
        self.host = host
        self.port = port

        self.server_tcp_init()

        self.vsrserver = VSR_Server()
        self.algo_th = Thread(target= self.vsrserver.run)
        self.algo_th.start()

        self.master = master_io()

        self.th_update = Thread(target= self.status_update)
        self.th_update.start()


    def server_tcp_init(self,):
        try:
            # if self.check_port_conflict():
            #     print(f"Port {self.port} has been used!!!")
            #     return False
            self.server = modbus_tcp.TcpServer(port=self.port,address=self.host,
                                               timeout_in_sec=5)
            self.server.start()
            self.register_init()
        except Exception as e:
            robot_logger.error(e,exc_info=True)

    def check_port_conflict(self,):
        try:
            ret = False
            sock = socket.socket(socket.AF_INET,socket.SOCK_STREAM)
            result = sock.connect_ex(self.host,self.port) # 检测有没有服务将这个端口以及IP地址占用
            if result == 0:
                ret = True
            else:
                ret = False
            sock.close()
            return ret
        except Exception as e:
            robot_logger.error(e,exc_info=True)
    
    def register_init(self,):
        self.slave =self.server.add_slave(1)  # 服务器上创建1号从机
        self.slave.add_block("robot6",cst.HOLDING_REGISTERS,0,400) # 创建400个保持寄存器
        self.slave.add_block("fifo",cst.HOLDING_REGISTERS,1024,100) # 创建50个保持寄存器用于存储FIFO
        self.slave.add_block("motor",cst.HOLDING_REGISTERS,10000,50) # 创建50个保持寄存器用于存储电机状态
        self.slave.add_block("di",cst.COILS,1024,50) # 创建50个线圈寄存器用于存储di
        self.slave.add_block("do",cst.COILS,20000,50) # 创建50个线圈寄存器用于存储d0

    def int2float(self,a,b):
        f=0
        try:
            z0=hex(a)[2:].zfill(4) #取0x后边的部分 右对齐 左补零
            z1=hex(b)[2:].zfill(4) #取0x后边的部分 右对齐 左补零
            z=z0+z1 #高字节在前 低字节在后
            f=struct.unpack('!f', bytes.fromhex(z))[0] #返回浮点数
        except BaseException as e:
            print(e)
        return f

    def float_to_int16s(self,f):
        # 将浮点数打包成32位二进制数据
        b = struct.pack('f', f)
        # 将32位二进制数据拆分成两个16位二进制数据
        i1, i2 = struct.unpack('HH', b)
        return i2, i1


    def status_update(self,):
        while True:
            try:
                time.sleep(0.005)
                ### 更新主机DI的状态
                m_di = []
                ### 共有13个DI
                for i in range (0,13):       
                    if self.master.get_DI(num=i+1) == True:
                        m_di.append(0)
                    else:
                        m_di.append(1)
                ### 获取DI并且刷新到MODBUS表中
                self.slave.set_values("robot6",70,m_di)   
                time.sleep(0.005)

                ### 更新机器人的状态
                status = []
                slider_status = []
                s = self.vsrserver.robot.status_global
                if s != None:
                    match s["state"]:
                        case "Idle":
                            status = status + [1]
                        case "Alarm":
                            status = status + [2]
                        case "Home":
                            status = status + [3]
                        case "Run":
                            status = status + [4]
                        case _:
                            status = status + [0]
                    ang = s["angle"]
                    for i in range(len(ang)):
                        a = self.float_to_int16s(ang[i])
                        status.append(a[0])
                        status.append(a[1])

                    ang = s["coordinate"]
                    for i in range(len(ang)):
                        a = self.float_to_int16s(ang[i])
                        status.append(a[0])
                        status.append(a[1])
                        if i ==6:
                            slider_status.append(a[0])
                            slider_status.append(a[1])
                    
                    ### 增加机器人的类型
                    status.append(self.vsrserver.robot.robot_type_number)     
                    ### 增加数字仿真服务状态
                    status.append(self.vsrserver.connect_flag)  
                    # status.append(self.vsrserver.robot.rtu_slave)

                    ### 将机器人的状态更新到0号寄存器
                    self.slave.set_values("robot6", 0, status)
                    ### 使用modbus进行通讯IOA 
                    self.slave.set_values("motor",10000,slider_status)


            except Exception as e:
                robot_logger.error(e,exc_info=True)

    ### 启动在线线程
    def online(self):
        file_path = '/home/cat/gm/controler/process_dis.py'
        try:

            if os.path.exists(file_path):
                self.online_p = subprocess.Popen(f'python3 {file_path}',shell=True,preexec_fn=os.setsid)
            else:
                raise Exception(f"No {file_path}")
        except Exception as e:
            print(e)
        # 启动在线线程

    def stop_process(self):
        ### 检查进程是否还在运行 
        if self.online_p and self.online_p.poll() is None:   
            try:
                ### 发送SIGTERM信号到进程组   
                os.killpg(os.getpgid(self.online_p.pid), signal.SIGTERM)   
            except ProcessLookupError:
                ### 进程可能已经不存在了    
                pass  

    def signal_handler(self, sig, frame):  
        self.stop_process()  
        # 这里可以添加更多的清理工作  

    def get_DO(self):
        try:
            values = [self.master.DO0.read(), self.master.DO1.read(), self.master.DO2.read(), self.master.DO3.read(), self.master.Motor_0.read()]  
            for i, val in enumerate(values):  
                self.vsrserver.IO[i] = int(val)

        except Exception as e:
            print(f"main drun get_DO error is {e}")

    def dorun(self,):
        self.online()
        signal.signal(signal.SIGINT, self.signal_handler) 
        Mdo_flag = [0,0,0,0,0]
        bo = 0x00
        while True:
            try:
                time.sleep(0.1)
                ### 1轮询执行DI的状态
                Mdo_list = self.slave.get_values("robot6", 86, 8)

                ### 两个电机口和三个LED灯
                for i in range(5):   
                    if Mdo_list[i] == Mdo_flag[i]:
                        pass
                    else:
                        Mdo_flag[i]= Mdo_list[i]
                        match Mdo_list[i]:
                            ### DO输出低电平
                            case 0:
                                self.master.set_LED(num=i,value=False)  
                            ### DO输出高电平
                            case 1:
                                self.master.set_LED(num=i,value=True)   


                ### 2执行DO的值
                self.get_DO()
                if Mdo_list[7] == 1:
                    self.master.set_DO(num = Mdo_list[5],value = Mdo_list[6])
                    ### 向IOA寄存器中写值
                    self.slave.set_values("do", 20000 + Mdo_list[5], Mdo_list[6])  
                    ### 将MODBUS表中的使能清除掉
                    self.slave.set_values("robot6", 93, 0)   

                config.set_value("BO0",self.master.bo_out)  

                ### 3执行机器人的指令
                cmd = self.slave.get_values("robot6", 49, 2)
                ### 查看49号寄存器值，P2P运动
                if cmd[0] == 1:
                    P = []
                    motion = self.slave.get_values("robot6", 31, 18)
                    for i in range(0, 14, 2):
                        P.append(self.int2float(motion[i],motion[i+1]))
                    self.vsrserver.robot.move(P= P, speed= motion[17], coordinate= motion[14], is_relative= motion[15], mode= motion[16])
                    self.slave.set_values("robot6", 49, 0)
                
                ### 复位等命令(查看50号寄存器值进行匹配)
                match cmd[1]:
                    case 0:
                        pass
                    case 1:
                        self.vsrserver.robot.home_1axis(1)
                        self.slave.set_values("robot6",50,0)
                        
                    case 2:
                        self.vsrserver.robot.home_1axis(2)
                        self.slave.set_values("robot6",50,0)
                        
                    case 3:
                        self.vsrserver.robot.home_1axis(3)
                        self.slave.set_values("robot6",50,0)
                        
                    case 4:
                        self.vsrserver.robot.home_1axis(4)
                        self.slave.set_values("robot6",50,0)
                    case 5:
                        self.vsrserver.robot.home_1axis(5)
                        self.slave.set_values("robot6",50,0)

                    case 6:
                        self.vsrserver.robot.home_1axis(6)
                        self.slave.set_values("robot6",50,0)

                    case 7:
                        self.vsrserver.robot.home_1axis(7)
                        self.slave.set_values("robot6",50,0)

                    case 8:
                        self.vsrserver.robot.home()
                        self.slave.set_values("robot6",50,0)

                    case 9:
                        self.vsrserver.robot.home_6axis_in_turn()
                        self.slave.set_values("robot6",50,0)

                    case 10:
                        self.vsrserver.robot.home_7axis()
                        self.slave.set_values("robot6",50,0)

                    case 11:
                        self.vsrserver.robot.unlock_all_axis()
                        self.slave.set_values("robot6",50,0)

                    case 12:
                        self.vsrserver.robot.stop()
                        self.slave.set_values("robot6",50,0)

                    case 13:
                        self.vsrserver.robot.go_to_zero()
                        self.slave.set_values("robot6",50,0)

            except Exception as e:
                robot_logger.error(e,exc_info=True)



class master_io():
    def __init__(self):
        self.gobal_do = 0

        self.A0 = 0
        self.A1 = 1
        self.A2 = 2
        self.A3 = 3
        self.A4 = 4
        self.A5 = 5
        self.A6 = 6
        self.A7 = 7

        self.B0 = 8
        self.B1 = 9
        self.B2 = 10
        self.B3 = 11
        self.B4 = 12
        self.B5 = 13
        self.B6 = 14
        self.B7 = 15      

        self.C0 = 16
        self.C1 = 17 
        self.C2 = 18
        self.C3 = 19 
        self.C4 = 20
        self.C5 = 21
        self.C6 = 22
        self.C7 = 23

        self.chip0 = "/dev/gpiochip0"  # chip0
        self.chip1 = "/dev/gpiochip1"  # chip1
        self.chip2 = "/dev/gpiochip2"  # chip2
        self.chip3 = "/dev/gpiochip3"  # chip3
        self.chip4 = "/dev/gpiochip4"  # chip4

        self.BUTTON1 = GPIO(self.chip1, self.A0, "in")  # 拨码按钮   
        self.BUTTON2 = GPIO(self.chip1, self.A1, "in")  # 离线按钮
        self.BUTTON3 = GPIO(self.chip1, self.A4, "in")  # 复位按钮

        self.DI_9 = GPIO(self.chip1, self.B2, "in")  # DI9 端口
        self.DI_8 = GPIO(self.chip1, self.B1, "in")  # DI8 端口

        self.DI_7 = GPIO(self.chip4, self.C3, "in")  # DI7 端口
        self.DI_6 = GPIO(self.chip4, self.C5, "in")  # DI6 端口
        self.DI_5 = GPIO(self.chip4, self.C2, "in")  # DI5 端口
        self.DI_4 = GPIO(self.chip3, self.B4, "in")  # DI4 端口
        self.DI_3 = GPIO(self.chip3, self.C1, "in")  # DI3 端口
        self.DI_2 = GPIO(self.chip3, self.B6, "in")  # DI2 端口
        self.DI_1 = GPIO(self.chip3, self.B5, "in")  # DI1 端口
        self.DI_0 = GPIO(self.chip1, self.B0, "in")  # DI0 端口

        self.Motor_0 = GPIO(self.chip3, self.B2, "out")  # 电机正转
        self.Motor_1 = GPIO(self.chip3, self.B3, "out")  # 电机反转
        self.LED2 = GPIO(self.chip3, self.A6, "out")  # 绿色
        self.LED3 = GPIO(self.chip3, self.A7, "out")  # 蓝色
        # self.LED4 = GPIO(self.chip4, self.C2, "out")  # 红色

        self.DO0 = GPIO(self.chip3, self.B1, "out")  # DO输出1
        self.DO1 = GPIO(self.chip3, self.B0, "out")  # DO输出2
        self.DO2 = GPIO(self.chip4, self.C6, "out")  # DO输出3
        self.DO3 = GPIO(self.chip4, self.C4, "out")  # DO输出4

        self.bo_out =0x00  

    def bo_count(self,BO,bo_num,value):# 定义一个按位计算的函数
        # BO 是一个8位的16进制数
        # bo_bit 是第几个IO
        # value 是DO的值
        # 将DO状态上传IOA服务器

        try:
            bit_num = 0b00000000
            if value ==0:
                match bo_num:
                    case 0 :
                        bit_num = 0b11111110
                    case 1 :
                        bit_num = 0b11111101

                    case 2 :
                        bit_num = 0b11111011
                    case 3 :
                        bit_num = 0b11110111
                    case 4 :
                        bit_num = 0b11101111
                    case 5 :
                        bit_num = 0b11011111
                    case 6 :
                        bit_num = 0b10111111
                    case 7 :
                        bit_num = 0b01111111
                BO  =  BO & bit_num
            
            elif value ==1:
                match bo_num:
                    case 0:
                        bit_num = 0b00000001
                    case 1:
                        bit_num = 0b00000010
                    case 2:
                        bit_num = 0b00000100
                    case 3:
                        bit_num = 0b00001000
                    case 4:
                        bit_num = 0b00010000
                    case 5:
                        bit_num = 0b00100000
                    case 6:
                        bit_num = 0b01000000
                    case 7:
                        bit_num = 0b10000000
                BO  =  BO | bit_num
            return BO            

        except Exception as e:
            print(e)

    def get_DI(self,num :int =1):
        try:
            value = -1

            match num:
                case 1:
                    value = self.BUTTON1.read()
                case 2:
                    value = self.BUTTON2.read()   
                case 3:
                    value = self.BUTTON3.read() 
                case 4:
                    value = self.DI_0.read() 
                case 5:
                    value = self.DI_1.read() 
                case 6:
                    value = self.DI_2.read() 
                case 7:
                    value = self.DI_3.read() 
                case 8:
                    value = self.DI_4.read() 
                case 9:
                    value = self.DI_5.read() 
                case 10:
                    value = self.DI_6.read() 
                case 11:
                    value = self.DI_7.read() 
                case 12:
                    value = self.DI_8.read() 
                case 13:
                    value = self.DI_9.read() 
              
            return value
        except Exception as e:
            robot_logger.error(e,exc_info=True) 

    # 这里是设置的四个灯的状态资源
    def set_LED(self,num,value=True):
        try:
            match num:
                case 0:
                    self.Motor_0.write(value)
                    if value:
                        self.bo_out = self.bo_count(self.bo_out,bo_num=6,value=1)
                    else:
                        self.bo_out = self.bo_count(self.bo_out,bo_num=6,value=0)
                case 1:
                    self.Motor_1.write(value)
                case 2:
                    self.LED2.write(value)
                case 3:
                    self.LED3.write(value)
                case 4:
                    pass
                    # self.LED4.write(value)


        except Exception as e:
            robot_logger.error(e,exc_info=True)

    #设置DO只需要调用这个函数
    def set_DO(self,num,value=1):
        try:
            value_bool = True
            if value == 0:
                value_bool =False
            elif value == 1:
                value_bool = True
            match num:
                case 0:
                    self.DO0.write(value_bool)
                case 1:
                    self.DO1.write(value_bool)
                case 2:
                    self.DO2.write(value_bool)
                case 3:
                    self.DO3.write(value_bool)

            self.bo_out = self.bo_count(self.bo_out,num,value=value)
        except Exception as e:
            robot_logger.error(e,exc_info=True)



class VSR_Server:
    def __init__(self) -> None:
        self.connect_flag = 0
        self.vsr_Server()
        self.robot = robot_control()

        self.robot_type = 0
        self.robot_state = None
        self.global_state = [1,1,1]
        ### IO 状态
        self.IO = [0, 0, 0, 0, 0, 0, 0, 0]
        self.default_speed = 500

        self.select_com_serial = False
        self.threads = []

        self.lock_object = threading.RLock()

    def vsr_Server(self):
        self.tcp_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
        self.tcp_socket.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, True)

        config.set_value(name="BO0",value=0)
        config.set_value(name="BO1",value=0) #初始化16个DO 
        speed_value=(self.float2bytes(75.0))
        config.set_value("FO[0]",value = speed_value)
        f01=(self.float2bytes(75.0))
        config.set_value("FO[1]",value=f01)

        self.tcp_socket.bind(('0.0.0.0', 8090))
        self.tcp_socket.listen(5)

    def float2bytes(self,f):
    #将浮点数打包成32位2进制数据
        b=struct.pack('<f',f)        
        return b[0],b[1],b[2],b[3]      

    def run(self):  
        try:  
            while True:  
                client_socket, client_addr = self.tcp_socket.accept()
                print(f"Connected by {client_addr}")
                # client_socket.setblocking(False)
                robot_logger.info(f"Connected by {client_addr}")  
                # 为每个客户端启动一个新的线程  

                client_thread = threading.Thread(target=self.handle_client, args=(client_socket, client_addr))  
                client_thread.start()  
                self.threads.append(client_thread)  

        except KeyboardInterrupt:  
            robot_logger.info("Server is shutting down...")  
            # 尝试优雅地关闭所有线程和套接字  
            self.tcp_socket.close()  
            for thread in self.threads:  
                thread.join()  # 等待所有线程完成  
  
    def handle_client(self, client_socket, client_addr):  
        try:  
            last_send_time = time.time()
            while True:
                
                current_time = time.time()
                    
                if current_time - last_send_time >= 0.05:
                    self.send_data_process(client_socket)
                    last_send_time = current_time

                try:
                    recv_data = client_socket.recv(1024, 0x40) 
                    if not recv_data: 
                        print("---- recv_data is None!!!!!!!!!!!") 
                        break  ### Client disconnected
    
                    recv_str = recv_data.decode('utf-8')  
                    if "A-Mode" in recv_str:  
                        time.sleep(0.005)
                        pass
                    else:  
                        time.sleep(0.005)  
                        self.parse_data(recv_str)
                
                except BlockingIOError as e:
                    recv_data = None
                    time.sleep(0.01)
        
        finally:  
            client_socket.close()  
            robot_logger.info(f"Disconnected {client_addr}")  

    def parse_data(self, recv_data):
        speed = 500
        self.step = 5
        index = recv_data.find('}')  
        if index != -1:  
            # 分割数据  
            # first_part = recv_data[:index+1]  
            second_part = recv_data[index+1:]  

        pattern_1 = r'"type":"([^"]*)","COM1":"([^"]*)"'
        pattern_2 = r'"type":"([^"]*)","COM2":"([^"]*)"'

       
        if "COM1" in second_part:
            # print(f"--- success in --COM1-")
            match = re.search(pattern_1, second_part)
            self.select_com_serial = False
        else:
            # print(f"--- success in ---COM2-")
            match = re.search(pattern_2, second_part)
            self.select_com_serial = True

        try:
            ## 检查是否是包含"type"和"C0M1"的JSON对象  
            if match:
                data_type = match.group(1)
                command = match.group(2)

                gcode = f"M50\n"
                # self.robot.serial_com1.write(gcode.encode('utf-8'))
                # self.robot.serial_com1.write(b"\r\n")s
                self.robot.send_msg(gcode, com_flag = 1)
                time.sleep(0.1)
                # self.robot.serial_com2.write(gcode.encode('utf-8'))
                # self.robot.serial_com2.write(b"\r\n")
                self.robot.send_msg(gcode, com_flag = 2)
                
                if data_type == 'M21':
                    if "up" in command:
                        gcode = f"!\n"
                        if self.select_com_serial:
                            self.robot.send_msg(gcode, com_flag = 2)
                        else:
                            self.robot.send_msg(gcode, com_flag = 1)
                        time.sleep(0.5)
                        gcode = f"%\n"
                    
                    elif "down" in command : 
                        # self.lock_object.acquire()
                        s_data = self.robot.status_global
                        # self.lock_object.release()

                        if s_data["state"] == "Alarm" and command != "restart":
                            self.lock_object.acquire()
                            data = "Robot's status is Alarm, Please Reset!"
                            send_data = {"return":data}
                            send_data = json.dumps(send_data)+"\r\n"
                            if self.select_com_serial:
                                self.robot.send_msg(gcode, com_flag = 2)
                            else:
                                self.robot.send_msg(gcode, com_flag = 1)
                            self.lock_object.release()
                            raise Exception("Please Reset!")
                        
                        angle_current = s_data["angle"]
                        angle_current = [int(i) for i in angle_current] 
                        
                        ## 根据不同的命令执行不同的操作
                        match command:
                            case "1_+_down" :
                                gcode = "M21 G90 G01 X160 Y{} Z{} A{} B{} C{} F{}\n".format(angle_current[1],angle_current[2],\
                                        angle_current[3],angle_current[4],angle_current[5], speed)
                            case "1_-_down" :
                                gcode = "M21 G90 G01 X-100 Y{} Z{} A{} B{} C{} F{}\n".format(angle_current[1],angle_current[2],\
                                        angle_current[3],angle_current[4],angle_current[5], speed)
                            case "2_+_down" :
                                gcode = "M21 G90 G01 X{} Y70 Z{} A{} B{} C{} F{}\n".format(angle_current[0],angle_current[2],\
                                        angle_current[3],angle_current[4],angle_current[5], speed)
                            case "2_-_down" :
                                gcode = "M21 G90 G01 X{} Y-35 Z{} A{} B{} C{} F{}\n".format(angle_current[0],angle_current[2],\
                                        angle_current[3],angle_current[4],angle_current[5], speed)
                            case "3_+_down" :
                                gcode = "M21 G90 G01 X{} Y{} Z60 A{} B{} C{} F{}\n".format(angle_current[0],angle_current[1],\
                                        angle_current[3],angle_current[4],angle_current[5], speed)
                            case "3_-_down" :
                                gcode = "M21 G90 G01 X{} Y{} Z-120 A{} B{} C{} F{}\n".format(angle_current[0],angle_current[1],\
                                        angle_current[3],angle_current[4],angle_current[5], speed)
                            case "4_+_down" :
                                gcode = "M21 G90 G01 X{} Y{} Z{} A180 B{} C{} F{}\n".format(angle_current[0],angle_current[1],\
                                        angle_current[2],angle_current[4],angle_current[5], speed)
                            case "4_-_down" :
                                gcode = "M21 G90 G01 X{} Y{} Z{} A-180 B{} C{} F{}\n".format(angle_current[0],angle_current[1],\
                                        angle_current[2],angle_current[4],angle_current[5], speed)
                            case "5_+_down" :
                                gcode = "M21 G90 G01 X{} Y{} Z{} A{} B29 C{} F{}\n".format(angle_current[0],angle_current[1],\
                                        angle_current[2],angle_current[3],angle_current[5], speed)
                            case "5_-_down" :
                                gcode = "M21 G90 G01 X{} Y{} Z{} A{} B-199 C{} F{}\n".format(angle_current[0],angle_current[1],\
                                        angle_current[2],angle_current[3],angle_current[5], speed)
                            case "6_+_down" :
                                gcode = "M21 G90 G01 X{} Y{} Z{} A{} B{} C349 F{}\n".format(angle_current[0],angle_current[1],\
                                        angle_current[2],angle_current[3],angle_current[4], speed)
                            case "6_-_down" :
                                gcode = "M21 G90 G01 X{} Y{} Z{} A{} B{} C-349 F{}\n".format(angle_current[0],angle_current[1],\
                                        angle_current[2],angle_current[3],angle_current[4], speed)
                        
                        self.lock_object.acquire()
                        if self.select_com_serial:
                            self.robot.send_msg(gcode, com_flag = 2)
                        else:
                            self.robot.send_msg(gcode, com_flag = 1)
                        time.sleep(0.006)
                        self.lock_object.release()
            
                
                elif data_type == 'M20':
                    
                    if "up" in command:
                        gcode = f"!\n"
                        if self.select_com_serial:
                            self.robot.send_msg(gcode, com_flag = 2)
                        else:
                            self.robot.send_msg(gcode, com_flag = 1)
                        time.sleep(0.5)
                        gcode = f"%\n"
                    
                    elif "down" in command : 
                        self.lock_object.acquire()
                        s_data = self.robot.status_global
                        self.lock_object.acquire()
                        time.sleep(0.01)

                        if s_data["state"] == "Alarm" and command != "restart":
                            self.lock_object.acquire()
                            data = "Robot's status is Alarm, Please Reset!"
                            send_data = {"return":data}
                            send_data = json.dumps(send_data)+"\r\n"
                            if self.select_com_serial:
                                self.robot.send_msg(gcode, com_flag = 2)
                                # self.robot.serial_com2.write(send_data.encode('utf-8'))
                                # self.robot.serial_com2.write(b"\r\n")
                            else:
                                self.robot.send_msg(gcode, com_flag = 1)
                                # self.robot.serial_com1.write(send_data.encode('utf-8'))
                                # self.robot.serial_com1.write(b"\r\n")
                            self.lock_object.release()
                            raise Exception("Please Reset!")
                        
                        angle_current = s_data["coordinate"]
                        angle_current = [int(i) for i in angle_current]          
                        ## 根据不同的命令执行不同的操作
                        match command:
                            case "1_+_down" :
                                gcode = f"M20 G91 G01 X{self.step} Y0 Z0 A0 B0 C00 F{speed}\n"
                            case "1_-_down" :
                                gcode = f"M20 G91 G01 X-{self.step} Y0 Z0 A0 B0 C00 F{speed}\n"
                            case "2_+_down" :
                                gcode = f"M20 G91 G01 X0 Y{self.step} Z0 A0 B0 C00 F{speed}\n"
                            case "2_-_down" :
                                gcode = f"M20 G91 G01 X0 Y-{self.step} Z0 A0 B0 C00 F{speed}\n"
                            case "3_+_down" :
                                gcode = f"M20 G91 G01 X0 Y0 Z{self.step} A0 B0 C00 F{speed}\n"
                            case "3_-_down" :
                                gcode = f"M20 G91 G01 X0 Y0 Z-{self.step} A0 B0 C00 F{speed}\n"
                            case "4_+_down" :
                                gcode = f"M20 G91 G01 X0 Y0 Z0 A{self.step} B0 C00 F{speed}\n"
                            case "4_-_down" :
                                gcode = f"M20 G91 G01 X0 Y0 Z0 A-{self.step} B0 C00 F{speed}\n"
                            case "5_+_down" :
                                gcode = f"M20 G91 G01 X0 Y0 Z0 A0 B{self.step} C00 F{speed}\n"
                            case "5_-_down" :
                                gcode = f"M20 G91 G01 X0 Y0 Z0 A0 B-{self.step} C00 F{speed}\n"
                            case "6_+_down" :
                                gcode = f"M20 G91 G01 X0 Y0 Z0 A0 B0 C{self.step} F{speed}\n"
                            case "6_-_down" :
                                gcode = f"M20 G91 G01 X0 Y0 Z0 A0 B0 C-{self.step} F{speed}\n"
                        
                        self.lock_object.acquire()
                        if self.select_com_serial:
                            self.robot.send_msg(gcode, com_flag = 2)
                        else:
                            self.robot.send_msg(gcode, com_flag = 1)
                        time.sleep(0.006)
                        self.lock_object.release()
             


                elif data_type == 'cmd':
                    if command is not None:
                        match command:
                            case "restart":
                                gcode = f"$h\r\n"
                                # print("复位操作")
                            
                            case "stop":
                                gcode = f"%\r\n"

                            case _:
                                pass

                    # print(f"robot---7-12 exec gcode is {gcode}")
                    self.lock_object.acquire()
                    if self.select_com_serial:
                        # time.sleep(0.91)
                        self.robot.send_msg(gcode, com_flag = 2)
                    else:
                        self.robot.send_msg(gcode, com_flag = 1)
                    time.sleep(0.006)
                    self.lock_object.release()
                
                self.lock_object.acquire()
                if self.select_com_serial:
                    self.robot.send_msg(gcode, com_flag = 2)
                else:
                    self.robot.send_msg(gcode, com_flag = 1)
                time.sleep(0.006)
                self.lock_object.release()
        
        except json.JSONDecodeError as e:  
            print(f"JSON解析错误: {e}")

    # 定义通讯协议格式  
    def build_packet(self,data_size, packet_type, message_body):  
        # 构建报头  
        header = json.dumps({  
            "data_size": data_size,  
            "type": packet_type  
        }).encode('utf-8')
        
        # 计算报头长度（4位bit，即半个字节，需要转换为字节长度）  
        header_length = len(header)
        
        # 构建帧头
        # frame_header = struct.pack('>H', header_length)
        frame_header = header_length.to_bytes(4, 'little')
        
        # 完整的数据包  
        packet = frame_header + header + message_body.encode('utf-8')
        return packet 

    # 定义消息体  
    def create_message_body(self,robot_type, state, com1, com2, rs485, com1Float, com2Float,io):  
        message_body = {  
            "robot_type": robot_type,  
            "state": state,  
            "com1": com1,  
            "com2": com2,  
            "RS485": rs485, 
            "com1Float": com1Float,
            "com2Float": com2Float,
            "IO": io  
        }
        data_str = json.dumps(message_body) + '\n'
        return data_str

    def robot_type_send(self):
        a = self.robot.robot_type_number 
        b = self.robot.robot_type_number_2
        c = "{}_{}".format(a, b)
        return c

    def send_data_process(self, client_socket):
        try:
            P = list()
            F = list()
            P.append(config.get_value("j0"))  
            P.append(config.get_value("j1"))
            P.append(config.get_value("j2"))
            P.append(config.get_value("j3"))
            P.append(config.get_value("j4"))
            P.append(config.get_value("j5"))
            
            F.append(config.get_value("j7"))
            F.append(config.get_value("j8"))
            F.append(config.get_value("j9"))
            F.append(config.get_value("j10"))
            
            if P is not None:
                P = P
            else:
                P = [0,0,0,0,0,0]
            
            if F is not None:
                F = F
            else:
                F = [0,0,0,0]
            

            robot_type = self.robot_type_send()
            state = self.global_state
            
            com1 = P
            com2 = F
            rs485 = ""
            com1Float = config.get_value("FO[1]")
            com2Float = config.get_value("FO[1]")
            io = self.IO
            
            ### 消息体
            message_body = self.create_message_body(robot_type, state, com1, com2, rs485, com1Float, com2Float, io)
            ### 消息体长度
            data_size = len(message_body)
            ### 控制器类型：实体控制器
            packet_type = "Physical" 
            ### 构建数据包 
            packet = self.build_packet(data_size, packet_type, message_body)
            # print(f"2024---7-11----send_data is {packet}")
            client_socket.sendall(packet)
    
        except Exception as e:
            print(f"send_process data is error is {e}")


server = Controler_Server()
server.dorun()